/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : triangulate.cc
 *@brief   : triangulate
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/9/6
 *
 */

#include "triangulator/triangulate.h"

using namespace triangulator;

Triangulate::Triangulate(const std::shared_ptr<FeatureDatabase> feature_database_ptr, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& extrinsic_matrix, const uint8_t& camera_id) : feature_database_ptr_(feature_database_ptr), intrinsic_matrix_(intrinsic_matrix), extrinsic_matrix_(extrinsic_matrix), camera_id_(camera_id) {
    triangulate_factor_ptr_ = std::make_shared<TriangulateFactor>(intrinsic_matrix, extrinsic_matrix);
}

const std::vector<Vector3d> Triangulate::TriangulateDLT(const std::map<uint64_t, Eigen::Matrix4d>& poses) {
    std::vector<Vector3d> world_points;
    std::unordered_map<uint64_t, std::shared_ptr<Feature>> feature_database = feature_database_ptr_->GetFeatureDatabase();
    for(const auto& feature : feature_database) {
        std::vector<uint64_t> frame_ids = feature.second->GetFrameIdsFromCameraId(camera_id_);
        if(frame_ids.size() <=1) {
            continue;
        }

        // LOG(INFO) << " frame_ids ===== : " << frame_ids.size() << std::endl;

        std::vector<Matrix4d> world_poses;
        // std::vector<Matrix4d> camera_poses;

        for(const uint64_t& frame_id : frame_ids) {
            world_poses.emplace_back(poses.at(frame_id));
            Matrix4d Tcw = TransformTwb2Tcw(poses.at(frame_id), extrinsic_matrix_);
            // camera_poses.push_back(Tcw);
        }
        std::vector<Vector2d> normalized_points;
        std::vector<Vector3d> normalized_point3ds;
        for(uint32_t idx = 0; idx < frame_ids.size(); ++idx) {
            std::unique_ptr<Vector3d> norm_point_ptr = feature.second->GetNormalizedPointFromCameraIdAndIndex(camera_id_, idx);
            if(norm_point_ptr == nullptr) {
                continue;
            }
            normalized_points.emplace_back((*norm_point_ptr).head<2>());
            normalized_point3ds.push_back(*norm_point_ptr);
        }
        if(normalized_points.empty()) {
            continue;
        } 
        Vector3d world_point = TriangulateDLTMethod(world_poses, normalized_points);
        // LOG(INFO) << " world_point ===== : " << world_point[0] << ", " << world_point[1] << ", " << world_point[2] << std::endl;

        if(!CheckInlier(world_point, world_poses, normalized_points, normalized_point3ds)) {
            continue;
        }

        // LOG(INFO) << " world_point ===== : " << world_point[0] << ", " << world_point[1] << ", " << world_point[2] << std::endl;

        // bool is_success = triangulate_factor_ptr_->OptimizeWorldPoints(normalized_points, world_poses, world_point);
        // if(!is_success) {
        //     continue;
        // }

        // LOG(INFO) << " after world_point ===== : " << world_point[0] << ", " << world_point[1] << ", " << world_point[2] << std::endl;

        // if(!CheckInlier(world_point, world_poses, normalized_points, normalized_point3ds)) {
        //     continue;
        // }
        
        // LOG(INFO) << " world_point ===== : " << world_point[0] << ", " << world_point[1] << ", " << world_point[2] << std::endl;

        feature_database_ptr_->UpdateLandmark(feature.first, feature.second, world_point);

        world_points.push_back(world_point);
    }
    return world_points;
}

const Vector3d Triangulate::TriangulateDLTMethod(const std::vector<Matrix4d>& pose, const std::vector<Vector2d>& normalized_points) {
    Vector3d world_point = Vector3d::Zero();

    const size_t num_obs = pose.size();
    Eigen::MatrixXd A(2 * num_obs, 4);
    Eigen::Matrix<double, 1, 4> P1, P2, P3;
    size_t idx = 0;
    for (size_t i = 0; i < num_obs; ++i) {
        const Vector2d &pt = normalized_points[i];
        const double x = pt[0];
        const double y = pt[1];
        // Matrix4d worlr_pose = pose[i];
        // Matrix4d camera_pose_in_world = worlr_pose * extrinsic_matrix_;
        // Matrix4d camera_pose = camera_pose_in_world.inverse();
        Matrix4d camera_pose = TransformTwb2Tcw(pose[i], extrinsic_matrix_);

        const Matrix3d &R = camera_pose.block<3,3>(0,0);
        const Vector3d &p = camera_pose.block<3,1>(0,3);

        P1 << R.block<1, 3>(0, 0), p(0);
        P2 << R.block<1, 3>(1, 0), p(1);
        P3 << R.block<1, 3>(2, 0), p(2);
        
        A.block<1, 4>(idx, 0) = x * P3 - P1;
        ++idx;
        A.block<1, 4>(idx, 0) = y * P3 - P2;
        ++idx;
    }

    bool is_use_scalar = true;
    double scalar = A.maxCoeff();

    if(!is_use_scalar) {
        scalar = 1;
    } 

    if(std::fabs(scalar) < 1e-12) {
        scalar = 1.0;
    }
    Matrix4d Ascalar = (1.0 / scalar) * Matrix4d::Identity();
    A = A * Ascalar;

    // LOG(INFO) << " --- scalar: " << scalar << "\n";

    // Solve Ax = 0.
    const Matrix4d AtA = A.transpose() * A;

    Eigen::JacobiSVD<Matrix4d> svd(AtA, Eigen::ComputeFullV);
    Vector4d S = svd.singularValues();
    Matrix4d V = svd.matrixV();
    Vector4d u4 = V.col(3);

    u4 = Ascalar.inverse() * u4;

    // LOG(INFO) << "S(2) and S(3): " << S(2) << ", " << S(3) << "\n";
    // LOG(INFO) << "S(2) / S(3): " << S(2) / S(3) << "\n";
    // LOG(INFO) << "u4(3,0): " << u4(3,0) << "\n";

    if((S(2) / S(3) > 1.5) && (std::fabs(u4(3,0)) > 1e-12)) {
        world_point(0) = u4(0,0) / u4(3,0);
        world_point(1) = u4(1,0) / u4(3,0); 
        world_point(2) = u4(2,0) / u4(3,0); 
    }
    return world_point;
}

bool Triangulate::CheckInlier(const Vector3d& world_point, const std::vector<Matrix4d>& pose, const std::vector<Vector2d>& normalized_points, const std::vector<Vector3d>& normalized_point3ds) {
    bool is_inlier = true;
    Vector3d init_ray_in_world;
    for(uint32_t pt_idx = 0; pt_idx < normalized_points.size(); ++pt_idx) {
        const Vector3d& normalized_pt3d = normalized_point3ds[pt_idx];

        Matrix4d Tcw = TransformTwb2Tcw(pose[pt_idx], extrinsic_matrix_);
        const Matrix3d &R = Tcw.block<3,3>(0,0);
        const Vector3d &t = Tcw.block<3,1>(0,3);
        Vector3d project_camera_point = R*world_point+t;

        // LOG(INFO) << " project_camera_point : " << project_camera_point << "\n";

        if(project_camera_point[2] < 0.0 || project_camera_point.norm() > 100.0) {
            is_inlier = false;
            // LOG(ERROR) << " project camera point is too large : " << project_camera_point.transpose() << "\n";
            break;
        }

        if(pt_idx == 0) {
            init_ray_in_world = R.transpose()*normalized_pt3d;
        } else {
            Vector3d ray_in_world = R.transpose()*normalized_pt3d;
            double base_line = (init_ray_in_world - ray_in_world).norm();
            if(base_line > 0.5) {
                // LOG(ERROR) << " two view ray distance is too large : " << base_line << "\n";
                break;
            }
            // LOG(INFO) << " base_line : " << base_line << "\n";

            double cos_disparity_angle = init_ray_in_world.dot(ray_in_world);
            if(cos_disparity_angle > 4.0) {
                // LOG(ERROR) << " two view ray disparity is too large : " << cos_disparity_angle << ", maybe two camera distance is far\n";
                break;
            }
            // LOG(INFO) << " cos_disparity_angle : " << cos_disparity_angle << "\n";
        } 

        Vector2d image_point(project_camera_point[0]/project_camera_point[2], project_camera_point[1]/project_camera_point[2]);
        Vector2d normalized_pt = normalized_points[pt_idx];
        Vector2d residual = NormalizedToImagePoint(normalized_pt) - NormalizedToImagePoint(image_point);
        if(residual.norm() > 1.0) {
            is_inlier = false;
            // LOG(ERROR) << " residual is too large : " << residual.transpose() << "\n";
            break;
        }

        // LOG(INFO) << " residual : " << residual.transpose() << "\n";
  
        if(!is_inlier) {
            break;
        }
    }
    return is_inlier;
}

Matrix4d Triangulate::TransformTwb2Tcw(const Matrix4d& Twb, const Matrix4d& Tbc) {
    Matrix4d Twc = Twb * Tbc;
    Matrix4d Tcw = Twc.inverse();
    return Tcw;
}

Eigen::Vector2d Triangulate::NormalizedToImagePoint(const Eigen::Vector2d& normalized_pt) {
    const float& fx = intrinsic_matrix_(0, 0);
    const float& fy = intrinsic_matrix_(1, 1);
    const float& cx = intrinsic_matrix_(0, 2);
    const float& cy = intrinsic_matrix_(1, 2);
    return {fx * normalized_pt(0) + cx, fy * normalized_pt(1) + cy};
}